// Copyright (c) 2012, Hazen Eckert
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//     * Neither the name of the The University of Texas at Dallas nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE UNIVERSITY OF TEXAS AT DALLAS BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include <ros/ros.h>
#include <auv_main_avr_driver/main_avr_driver.h>
#include <auv_main_avr_driver/main_avr_driver_node.h>
#include <diagnostic_updater/diagnostic_updater.h>
#include <dynamic_reconfigure/server.h>
#include <auv_main_avr_driver/auvConfig.h>
#include <auv_msgs/Depth.h>
// #include <auv_msgs/SetMarkerDropper.h>

main_avr_driver_node::main_avr_driver_node(ros::NodeHandle h):auv_driver_node(h), server()
{
    state = auv_driver_node::CLOSED;
    
    driver = new main_avr_driver;
    
  	dynamic_reconfigure::Server<auv_main_avr_driver::auvConfig>::CallbackType f;
  	f = boost::bind(&main_avr_driver_node::reconfigureCallback, this, _1, _2);
    server.setCallback(f);
    
    private_nh.param<std::string>("port", port, "/dev/main_avr");
    private_nh.param<int>("Km", Km, -187);
    private_nh.param<int>("Kb", Kb, 170402);
    
    try {
    	driver->open(port.c_str(), 115200);
    	state = auv_driver_node::OPENED;
    	getDriverInfo();
 /*       if (((main_avr_driver *)driver)->setMarkerDropper(0) != 0) {
            ROS_ERROR("Set marker dropper failed");
        }*/
    } catch (int e) {
    	if (e == 19){
    		ROS_ERROR("Open FAILED");
    	}
    	state = auv_driver_node::CLOSED;
    }
    
    updater.add("Power Diagnostics", (main_avr_driver_node *)this, &main_avr_driver_node::update_power_diagnostics);
    updater.add("Standard Diagnostics", (auv_driver_node *)this, &auv_driver_node::update_standard_diagnostics);
    
    depth_pub = nh.advertise<auv_msgs::Depth>("depth", 10);
    
}

main_avr_driver_node::~main_avr_driver_node()
{
    delete driver;
}

void main_avr_driver_node::reconfigureCallback(auv_main_avr_driver::auvConfig &config, uint32_t level)
{
    if (((std::string)config.port_name).compare(port) != 0) {
        if (state > auv_driver_node::CLOSED) {
       		driver->close();
       		state = auv_driver_node::CLOSED;
       	}
       	
        try {
            driver->open(((std::string)config.port_name).c_str(), 115200);
            state = auv_driver_node::OPENED;
            getDriverInfo();
            /*if (((main_avr_driver *)driver)->setMarkerDropper(0) != 0) {
                ROS_ERROR("Set marker dropper failed");
            }*/
        } catch (int e) {
            if (e == 19){
                ROS_ERROR("Open FAILED");
            }
            state = auv_driver_node::CLOSED;
        }
    }
    
    Km = config.Km;
    Kb = config.Kb;
}

void main_avr_driver_node::spin()
{
    ros::Rate loop_rate(50);
    while (!ros::isShuttingDown()) {
        if (!driver->isOpened()) {
            state = auv_driver_node::CLOSED;
        } else {
            uint8_t depth;
            if (((main_avr_driver *)driver)->getSensorData(&depth, &battery, &killSwitch) != 0) {
                ROS_ERROR("Get Sensor data failed");
            } else {
                auv_msgs::Depth msg;
                int realDepth = 0;
                realDepth |= (depth<<2);
                realDepth |= (0x03 & killSwitch);
                realDepth = Km*realDepth + Kb;
                msg.depth = realDepth;
                depth_pub.publish(msg);
            }
        }
        spinOnce();
        loop_rate.sleep();
    }
}

void main_avr_driver_node::update_power_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
{
    if ((killSwitch & 0x80) != 0) {
        stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "AUV Main AVR Driver Power Diagnostics");
        int realBattery = 0;
        realBattery |= (battery<<2);
        realBattery |= (0x03 & (killSwitch>>2));
        double const dInvBattRatio=(5.0/1024)*(38.9/5.9); // ADC*VDivider
        double battV = realBattery*dInvBattRatio*1000;
        stat.add("Battery", battV);
        stat.add("Kill Switch", "PULLED");
    } else {
    		stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "AUV Main AVR Driver Power Diagnostics");
        int realBattery = 0;
        realBattery |= (battery<<2);
        realBattery |= (0x03 & (killSwitch>>2));
        double const dInvBattRatio=(5.0/1024)*(38.9/5.9); // ADC*VDivider
        double battV = realBattery*dInvBattRatio*1000;
        stat.add("Battery", battV);
        stat.add("Kill Switch", "OK");
    }
}
/*
bool main_avr_driver_node::setMarkerDropper(auv_main_avr_driver::SetMarkerDropper::Request &req, auv_main_avr_driver::SetMarkerDropper::Response &res)
{
    if (req.position < 0 || req.position > 2) {
        res.success = false;
    } else {
        if (((main_avr_driver *)driver)->setMarkerDropper() != 0) {
            ROS_ERROR("Set marker dropper failed");
            res.success = false;
        } else {
            res.success = true;
        }
    }
    return true;
}*/

int main(int argc, char **argv)
{
    ros::init(argc, argv, "auv_main_avr_driver");
    
    ros::NodeHandle n;

    main_avr_driver_node node(n);
    
    node.spin();

    return 0;
}
